package graph.entities;

import static ogl.vecmathimp.FactoryDefault.vecmath;
import graph.Entity;
import graph.Part;
import ogl.vecmath.Matrix;
import ogl.vecmath.Vector;
import ogl.vecmathimp.MatrixImp;

public class Walk extends Entity {

	float rotUp = 0f;
	float rotDown = 0f;
	float rotLeft = 0f;
	float rotRight = 0f;

	boolean first = true;

	public Walk(Part p) {
		super(p);
	}

	@Override
	public void simulate(EntityType et, float elapsed) {
		MatrixImp m = super.getPart().getMatrix();
		MatrixImp modelMatrix = m.getTranslation();

		float angleX = (float) Math.atan2(m.get(0, 2), m.get(1, 2));
		// float angleY = (float) Math.atan2(-m.get(0, 2),
		// Math.sqrt(Math.pow(m.get(1, 2), 2) + Math.pow(m.get(2, 2), 2)));
		float angleY = (float) Math.acos(m.get(2, 2));
//		System.out.println("X: " + angleX);
//		System.out.println("Y: " + angleY);

		if (first) {
			if (angleX > 0)
				rotLeft = angleX;
			else
				rotRight = angleX;

			if (angleY > 0)
				rotUp = angleY;
			else
				rotDown = angleY;
			first = false;
		}

		Matrix translationMatrix = vecmath.translationMatrix(0f, 0f, 0f);

		if (et == EntityType.WalkRight) {
			translationMatrix = vecmath.translationMatrix(elapsed * 2, 0, 0);
		} else if (et == EntityType.WalkLeft) {
			translationMatrix = vecmath.translationMatrix(-elapsed * 2, 0, 0);
		} else if (et == EntityType.WalkBackward) {
			translationMatrix = vecmath.translationMatrix(0, 0, elapsed * 2);
		} else if (et == EntityType.WalkForward) {
			translationMatrix = vecmath.translationMatrix(0, 0, -elapsed * 2);
		} else if (et == EntityType.RotateCamUp) {
			rotUp += elapsed;
		} else if (et == EntityType.RotateCamDown) {
			rotDown += elapsed;
		} else if (et == EntityType.RotateCamRight) {
			rotRight += elapsed;
		} else if (et == EntityType.RotateCamLeft) {
			rotLeft += elapsed;
		}

		modelMatrix = (MatrixImp) modelMatrix
				.mult(vecmath.rotationMatrix(0, 1, 0, rotRight - rotLeft))
				.mult(translationMatrix)
				.mult(vecmath.rotationMatrix(1, 0, 0, rotUp - rotDown));

		this.getPart().setMatrix(modelMatrix);
	}

}
